#!/usr/bin/env python3

#Copyright (c) 2025 Huawei Device Co., Ltd. All rights reserved.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import cv2
import numpy as np
from rclpy.qos import QoSProfile
import os

class ApplePublisher(Node):
    def __init__(self):
        super().__init__('apple_publisher')
        qos_profile = QoSProfile(depth=10)
        self.publisher_ = self.create_publisher(Image, 'apple_image_topic', qos_profile)
        
        # 设置图片文件夹路径
        self.image_dir = "apple_images"
        os.makedirs(self.image_dir, exist_ok=True)
        self.image_files = [f for f in os.listdir(self.image_dir) 
                          if f.endswith(('.jpg', '.png'))]
        self.current_index = 0
        
        self.timer = self.create_timer(1.0, self.publish_image)
        self.get_logger().info(f"Apple publisher started, found {len(self.image_files)} images")

    def publish_image(self):
        if not self.image_files:
            self.get_logger().warn("No images found in directory")
            return
            
        # 读取图片
        img_path = os.path.join(self.image_dir, self.image_files[self.current_index])
        image = cv2.imread(img_path)
        
        if image is None:
            self.get_logger().error(f"Failed to load image: {img_path}")
            return
        
        # 转换为ROS消息
        msg = Image()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "camera_frame"
        msg.height = image.shape[0]
        msg.width = image.shape[1]
        msg.encoding = 'bgr8'
        msg.is_bigendian = False
        msg.step = image.shape[1] * 3
        msg.data = image.tobytes()
        
        self.publisher_.publish(msg)
        self.get_logger().info(f"Published: {img_path}")
        
        # 更新索引
        self.current_index = (self.current_index + 1) % len(self.image_files)

def main(args=None):
    rclpy.init(args=args)
    publisher = ApplePublisher()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()